#include "QuakeCamera.h"
#include "MouseManager.h"
#include "KeyboardManager.h"
#include "JoystickManager.h"
#include "MatrixHelper.h"
#include "MathHelper.h"
#include "GraphicsDeviceManager.h"

QuakeCamera::QuakeCamera()
{
	m_fPitch = 0;
	m_fYaw = 0;
	m_fRoll = 0;
	m_fSpeedRamp = 0;
}

QuakeCamera::~QuakeCamera()
{
}

void QuakeCamera::Update(float elapsedTime)
{
	GraphicsDeviceManager* graphicsDeviceManager = GraphicsDeviceManager::GetInstance();
	KeyboardManager* keyboardManager = KeyboardManager::GetInstance();
	MouseManager* mouseManager = MouseManager::GetInstance();

	Vector2i mousePosition = mouseManager->GetPosition();
	Vector2i viewportCenter = graphicsDeviceManager->GetViewport().Rectangle.GetCenter();
	Quaternion rotation;
	Matrix matrix;
	Vector3f right, up, forward;

	if(mousePosition != viewportCenter)
	{
		Vector2i mousePositionChange = mouseManager->GetPositionChange();

		if(mousePositionChange.X != 0)
		{
			m_fYaw += DegreesToRadians(mousePositionChange.X * QUAKECAMERA_SENSITIVITY);
		}

		if(mousePositionChange.Y != 0)
		{
			m_fPitch += DegreesToRadians(mousePositionChange.Y * QUAKECAMERA_SENSITIVITY);
		}

		Clamp(m_fPitch, QUAKECAMERA_PITCH_MINIMUM, QUAKECAMERA_PITCH_MAXIMUM);
		
		rotation *= Quaternion::CreateFromAxisAngle(Vector3f::Forward, m_fRoll);	// TODO: Verify roll correctness.
		rotation *= Quaternion::CreateFromAxisAngle(Vector3f::Left, m_fPitch);
		rotation *= Quaternion::CreateFromAxisAngle(Vector3f::Up, m_fYaw);

		SetRotation(rotation);
	}

	MatrixHelper::SetRotation(matrix, GetRotation());
	MatrixHelper::GetRight(matrix, right);
	MatrixHelper::GetUp(matrix, up);
	MatrixHelper::GetForward(matrix, forward);

	Vector3f velocity;

	// TODO: Change to message polling instead of direct input polling.
	if(keyboardManager->IsKeyDown('W'))
	{
		velocity += forward;
	}
	
	if(keyboardManager->IsKeyDown('S'))
	{
		velocity -= forward;
	}

	if(keyboardManager->IsKeyDown('A'))
	{
		velocity += right;
	}
	
	if(keyboardManager->IsKeyDown('D'))
	{
		velocity -= right;
	}

	if(velocity == Vector3f::Zero)
	{
		if(m_fSpeedRamp != 0)
		{
			m_fSpeedRamp -= (1 / QUAKECAMERA_SPEEDRAMPTIME) * elapsedTime;

			Clamp(m_fSpeedRamp, 0.0f, 1.0f);
		}

		velocity = GetVelocity() * (1 - QUAKECAMERA_DAMPING * elapsedTime);
	}
	else	
	{
		if(m_fSpeedRamp != 1)
		{
			m_fSpeedRamp += (1 / QUAKECAMERA_SPEEDRAMPTIME) * elapsedTime;

			Clamp(m_fSpeedRamp, 0.0f, 1.0f);
		}

		velocity *= QUAKECAMERA_SPEED_MAXIMUM * m_fSpeedRamp;
	}
	
	SetVelocity(velocity);

	Camera::Update(elapsedTime);
}